/*
 * (C) Copyright 2006 Marvell International Ltd.
 * All Rights Reserved
 */

#include <arch.h>
#include <types.h>
#include <nandflash.h>
#include <xllp_dfc.h>
#include <serial.h>
#include <errno.h>
#include <util.h>

int printf(const char *fmt, ...);
//#define DEBUG_NAND

#ifdef DEBUG_NAND

#define DPRINTF(fmt,args...) printf( fmt, ##args )

#define PRINT_BUF(buf, num)  print_buf((buf), (num))
#else

#define DPRINTF(fmt,args...)
#define PRINT_BUF(buf, num)
#endif

/* The maxium block number is 8096. So the maxium bbt length is 2048 bytes */
#define BBT_LEN     2048

#if defined(TAVOR_EVB2)
#define NAND_RELOC_MAX  20
#else
#define NAND_RELOC_MAX  127
#endif

#define NAND_RELOC_HEADER   0x0000524e //= 'N' + ('R' * 256)

/* The bad block table is in the last good block of the device */
#define NAND_BBT_LASTBLOCK  0x00000010
/* bbt has a version counter at offset veroffs */
#define NAND_BBT_VERSION    0x00000100

#define MAX_TIM_SIZE    0x1000
#define MAX_BBT_SLOTS   24

#define BBT_BLOCK_BAD   0x03
#define BBT_BLOCK_GOOD  0x00
#define BBT_BLOCK_REV1  0x01
#define BBT_BLOCK_REV2  0x02

#define ERR     0x100
#define EBADBLOCK   (ERR + 1)
#define ECMDFAIL    (ERR + 2)
#define ENOSPACE    (ERR + 3)
#define ERELOCATED      (ERR + 4)

#define BUFLEN      (2048 + 64)

#define READ_NORM       0
#define READ_OOB        1
#define READ_RAW        2
enum {
    MHN_OBM_NULL = 0,
    MHN_OBM_V1,
    MHN_OBM_V2,
    MHN_OBM_V3,
    MHN_OBM_INVAL
};

struct nand_rel_table{
    u16 header;
    u16 total;
    struct {
        u16 from;
        u16 to;
    } rel[NAND_RELOC_MAX];
};

struct nand_bbt_descr {
    int options;
    int pages;
    int offs;
    int veroffs;
    u8  version;
    int len;
    int     maxblocks;
    int reserved_block_code;
    u8  *pattern;
};

struct nand_info {
    u8 maf_id;
    u8 dev_id;
    u32 chipsize;
    int page_shift;
    int phys_erase_shift;
    int eccbytes;
    u32 oobsize;
    u32 hwecc;
    u8 bbt[BBT_LEN];
    u32 badblockpos;
    u32 badblockbytes;
    struct nand_bbt_descr *td;
    struct nand_bbt_descr *md;
    /* current slot in relocation table */
    s32 current_slot;
    XLLP_DFC_CONTEXT_T *pcontext;
    struct nand_rel_table table;
    unsigned int table_init;
};
inline int is_bad_block(u32 block, struct nand_info *info);
int mhn_erase_one_block(struct nand_info * info, u32 block);
int mhn_write_one_page(struct nand_info *info, u8 * buf, u32 page, int hwecc);
int mhn_read_one_page(struct nand_info * info, u8 *buf, u32 page, int option);

typedef unsigned char u_char;

typedef enum {
    NULLFUNC = 0,
    ZIMAGE = 1,
    JFFS2 = 2,
    YAFFS = 3
}WRITE_FUNC;

static XLLP_DFC_MODE_T DFC_mode = {
    .enableDMA = 0,
    .enableECC = 1,
    .enableSpareArea = 1,
    .chipSelect = 0,
};

static XLLP_DFC_CONTEXT_T DFC_context = {
    .pDFCReg = (P_XLLP_DFC_REG_T)(0x43100000),
    .pDFCMode = &DFC_mode,
    .pDma = NULL,
    .pMFPBase = (P_XLLP_VUINT32_T)NULL,
    .pMFPRmDb = (P_XLLP_MFP_RM_DB_ID_T)NULL,
    .pOSTReg = (P_XLLP_OST_T)(0x40A00000),
    .pDmacRegsBase = NULL,
    .pFlashInfo = NULL,
};

unsigned int flash_config = XLLP_DFC_FLASH_NULL;

static struct nand_info monahans_nand;
static u8 jffs2_sb_oob[16] = {0xFF, 0xFF, 0x85, 0x19, 0x3, 0x20, 0x0, 0x0, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
static u8 jffs2_lb_oob[64] = {0xFF, 0xFF, 0x85, 0x19, 0x3, 0x20, 0x0, 0x0, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};

/* These two patterns are used to check BBT */
static u8 scan_main_bbt_pattern[] = { 'p', 'x', 'a', '1'};
static struct nand_bbt_descr monahans_bbt_main = {
    .options = NAND_BBT_LASTBLOCK | NAND_BBT_VERSION,
    .veroffs = 6,
    .maxblocks = 2,
    .len = 4,
    .offs = 2,
    .pattern = scan_main_bbt_pattern,
};

static u8 scan_mirror_bbt_pattern[] = { '0', 'a', 'x', 'p'};
static struct nand_bbt_descr monahans_bbt_mirror = {
    .options = NAND_BBT_LASTBLOCK | NAND_BBT_VERSION,
    .veroffs = 6,
    .maxblocks = 2,
    .len = 4,
    .offs = 2,
    .pattern = scan_mirror_bbt_pattern,
};

static unsigned char buf[BUFLEN];
static int write_bbt (struct nand_info *info, u8 *buf, struct nand_bbt_descr *td, struct nand_bbt_descr *md);
static inline void set_bbt(u32 block, u8 val, struct nand_info *info);
static int mark_badblock(u32 block, struct nand_info *info);
static int sync_rel_table(struct nand_info *info, int *idx);

void print_buf(char *buf, int num)
{
    int i = 0;
    while (i < num) {
        printf("%08x: %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x\n",
                buf+i,
                *((char *)(buf + i)),
                *((char *)(buf + i + 1)),
                *((char *)(buf + i + 2)),
                *((char *)(buf + i + 3)),
                *((char *)(buf + i + 4)),
                *((char *)(buf + i + 5)),
                *((char *)(buf + i + 6)),
                *((char *)(buf + i + 7)),
                *((char *)(buf + i + 8)),
                *((char *)(buf + i + 9)),
                *((char *)(buf + i + 10)),
                *((char *)(buf + i + 11)),
                *((char *)(buf + i + 12)),
                *((char *)(buf + i + 13)),
                *((char *)(buf + i + 14)),
                *((char *)(buf + i + 15)));
        i += 16;
    }
}

/*
 * MOBM V2 is used on MH-P B0/B1/B2, MH-PL B1, MH-L A0.
 * MOBM V3 is used on MH-LV A0, ...
 */
static int calc_obm_ver(void)
{
    unsigned int    cpuid;
    /* read CPU ID */
    __asm__ (
        "mrc p15, 0, %0, c0, c0, 0\n"
        : "=r" (cpuid)
    );
    if ((cpuid & 0xFFFF0000) != 0x69050000) {
        /* It's not xscale chip. */
        return MHN_OBM_INVAL;
    }
    if ((cpuid & 0x0000FFF0) == 0x00006420) {
        /* It's MhnP Ax */
        return MHN_OBM_V2;
    }
    if ((cpuid & 0x0000FFF0) == 0x00006820) {
        /* It's MhnP Bx */
        if ((cpuid & 0x0F) <= 6)
            return MHN_OBM_V2;
        else
            return MHN_OBM_V3;
    }
    if ((cpuid & 0x0000FFF0) == 0x00006880) {
        /* It's MhnL Ax */
        if ((cpuid & 0x0F) == 0)
            return MHN_OBM_V2;
        else
            return MHN_OBM_V3;
    }
    if ((cpuid & 0x0000FFF0) == 0x00006890) {
        /* It's MhnLV Ax */
        return MHN_OBM_V3;
    }
    if ((cpuid & 0x0000FFFF) == 0x00006834) {
        /* It's Tavor A0 */
        return MHN_OBM_V2;
    }
    if ((cpuid & 0x0000FFFF) == 0x00006835) {
        /* It's Tavor B0 */
        return MHN_OBM_V3;
    }
    return MHN_OBM_INVAL;
}

static void dump_rel_table(struct nand_rel_table *table)
{
    int i, max;

    if (table->total > NAND_RELOC_MAX)
        max = NAND_RELOC_MAX;
    else
        max = table->total;
    DPRINTF("Header: 0x%x, Total: %d\n", table->header, table->total);
    for (i = 0; i < max; i++) {
        printf("Block #%d is relocated to #%d\n", table->rel[i].from,
            table->rel[i].to);
    }
}

static int update_rel_table(struct nand_info *info, int block);

/*
 * MOBM maintains a relocation table. It's used to replace bad blocks.
 * If block A is bad, it will use block B instead.
 * There're 127 relocated blocks. All of them reside in the bottom of NAND
 * flash. So they're reserved and can't be calculated in mtd size and chip
 * size.
 */
static int read_rel_table(struct nand_info *info)
{
    struct nand_rel_table *table = NULL;
    int page, pagesize, maxslot;
    int obm, valid, ret;
    //unsigned char buf[BUFLEN];

    obm = calc_obm_ver();
    memset(buf, 0, BUFLEN);

    page = 1 << (info->phys_erase_shift - info->page_shift);
    pagesize = 1 << info->page_shift;

    valid = 0;
    if (obm == MHN_OBM_V2) {
        /* On MOBM V2, the relocation table resides in the last page
         * of the first block.
         * Hardware ECC isn't necessary at this time.
         */
        page = (1 << (info->phys_erase_shift - info->page_shift)) - 1;
        ret = mhn_read_one_page(info, buf, page, READ_NORM);
        memcpy(((unsigned char *)&(info->table)), buf,
            sizeof(struct nand_rel_table));
        if (info->table.header == NAND_RELOC_HEADER)
            valid = 1;
    } else if (obm == MHN_OBM_V3) {
        /* On MOBM V3, there're several relocation tables in the first
         * block.
         * When new bad blocks are found, a new relocation table will
         * be generated and written back to the first block. But the
         * original relocation table won't be erased. Even if the new
         * relocation table is written wrong, system can still find an
         * old one.
         * One page contains one slot.
         */
        maxslot = 1 << (info->phys_erase_shift - info->page_shift);
        page = maxslot - MAX_BBT_SLOTS;
        for (; page < maxslot; page++) {
            ret = mhn_read_one_page(info, buf, page, READ_NORM);
            table = (struct nand_rel_table *)buf;
            if (ret == 0) {
                if (table->header != NAND_RELOC_HEADER) {
                    continue;
                } else {
                    info->current_slot = maxslot - page - 1;
                    printf("Slot %d Found\n", info->current_slot);
                    memcpy(((unsigned char *)&(info->table)),
                        table, sizeof(struct nand_rel_table));
                    valid = 1;
                    break;
                }
            }
        }
    } else {
        printf("The version of MOBM isn't supported\n");
    }
    if (valid) {
        memcpy(((unsigned char *)&(info->table)), buf,
            sizeof(struct nand_rel_table));
        if (info->table.total > NAND_RELOC_MAX)
            info->table.total = NAND_RELOC_MAX;
        info->table_init = 1;
        dump_rel_table(&(info->table));
    } else {
        /* There should be a valid relocation table slot at least. */
        printf("NO VALID relocation table can be recognized\n");
        printf("CAUTION: It may cause unpredicated error\n");
        printf("Please re-initialize the NAND flash.\n");
        memset((unsigned char *)&(info->table), 0,
                sizeof(struct nand_rel_table));
        info->table_init = 0;
        return -EINVAL;
    }
    return 0;
}

/* add the relocation entry into the relocation table
 * It's valid on MOBM V3.
 */
static int update_rel_table(struct nand_info *info, int block)
{
    unsigned long  bad_block = 0xFFFFFFFF;
    struct nand_rel_table *table = NULL;
    XLLP_STATUS_T status;
    int obm, reloc_block, i;
    //u8 buf[BUFLEN];

    if (block >= (info->chipsize >> info->phys_erase_shift))
        return -EBADBLOCK;

    obm = calc_obm_ver();
    if ((obm == MHN_OBM_V3) || (obm == MHN_OBM_V2 && !info->table_init)) {
        table = &info->table;
        if (info->table_init == 0) {
            printf("Error: the initial relocation table can't be read\n");
            memset(table, 0, sizeof(struct nand_rel_table));
            table->header = NAND_RELOC_HEADER;
            info->table_init = 1;
        }
        for (i = 0; i < table->total; i++) {
            if (table->rel[i].from == block)
                return 1;
        }
        if (table->total == 0) {
            /* Point to the first relocated block.
             * It resides in the last block of flash.
             * the relocation entry has calculated in
             * chipsize
             */
            reloc_block = (info->chipsize
                    >> info->phys_erase_shift)
                    + NAND_RELOC_MAX - 1;
        } else if (table->total < NAND_RELOC_MAX) {
            reloc_block = table->rel[table->total - 1].to - 1;
        } else {
            DPRINTF("Relocation table exceed max number, ");
            DPRINTF("cannot relocate block 0x%x\n", block);
            return -ENOSPACE;
        }
        /* Make sure that reloc_block is pointing to a valid block */
        for (; ; reloc_block--) {
            /* The relocate table is full */
            if (reloc_block < (info->chipsize >> info->phys_erase_shift))
                return -ENOSPACE;
            status = XllpFlashDfcErase(info->pcontext, reloc_block,
                            1, NAND_CMD_TIMEOUT, &bad_block);
            if (status == XLLP_STATUS_SUCCESS)
                break;
            else {
                DPRINTF("status:0x%x found at erasing reloc block %d\n", reloc_block);
                /* skip it if the reloc_block is also a
                 * bad block
                 */
                if (bad_block == reloc_block) {
                    reloc_block--;
                } else
                    return -ECMDFAIL;
            }
        }
        /* Create the relocated block information in the table */
        table->rel[table->total].from = block;
        table->rel[table->total].to = reloc_block;
        table->total++;
    }else{
        return -ENOSPACE;
    }
    return 0;
}

/* Write the relocation table back to device, if there's room. */
static int sync_rel_table(struct nand_info *info, int *idx)
{
    int obm, start_page, len;

    if (*idx < -1) {
        DPRINTF("Wrong Slot is specified.\n");
        return -1;
    }
    obm = calc_obm_ver();
    if ((obm == MHN_OBM_V3 && (*idx + 1) >= MAX_BBT_SLOTS) ||
        (obm == MHN_OBM_V2 && (*idx + 1) >= 1)){
        printf("Can't write relocation table to device any more.\n");
        return -1;
    }

    if (info->table_init == 0) {
        memset(&info->table, 0, sizeof(struct nand_rel_table));
        info->table.header = NAND_RELOC_HEADER;
        info->table_init = 1;
    }

    *idx += 1;
    len = 4;
    len += info->table.total << 2;
    /* write to device */
    start_page = 1 << (info->phys_erase_shift - info->page_shift);
    start_page = start_page - 1 - *idx;
    memset(buf, 0xFF, BUFLEN);
    memcpy(buf, &(info->table), len);

    DPRINTF("Write relocation table to slot %d\n", *idx);
    DPRINTF("write relocation table to page:0x%x\n", start_page);
    mhn_write_one_page(info, buf, start_page, 1);
#if 0
    /* dump it */
    memset(buf, 0, BUFLEN);
    mhn_read_one_page(info, buf, start_page, READ_OOB);
    PRINT_BUF(buf, len);
#endif

    return 0;
}

/* Find the relocated block of the bad one.
 * If it's a good block, return 0. Otherwise, return a relocated one.
 * idx points to the next relocation entry
 */
static unsigned short search_rel_block(int block, struct nand_info *info)
{
    struct nand_rel_table *table = NULL;
    int max, i, reloc_block = 0;

    if (info->table_init == 0)
        return 0;

    table = &info->table;
    if ((block <= 0) || (block >= (info->chipsize >> info->phys_erase_shift))
            || (info->table_init == 0) || (table->total == 0))
        return 0;
    if (table->total > NAND_RELOC_MAX)
        max = NAND_RELOC_MAX;
    else
        max = table->total;
    for (i = 0; i < max; i++) {
        if (block == table->rel[i].from) {
            reloc_block = table->rel[i].to;
        }
    }
    return reloc_block;
}

static void inline enable_hw_ecc(int enable, struct nand_info *info)
{
    if (!enable) {
        NDCR &= ~NDCR_ECC_EN;
        info->pcontext->pDFCMode->enableECC = 0;
    } else {
        NDCR |= NDCR_ECC_EN;
        info->pcontext->pDFCMode->enableECC = 1;
    }
}

/* check whether a block is a bad block.
 * If this block is registered in relocation table, check the relocated
 * one.
 */
inline int is_bad_block(u32 block, struct nand_info *info)
{
    struct nand_bbt_descr *td = info->td;
    struct nand_bbt_descr *md = info->md;
    int numblocks;
    int bbt_start, bbt_end;

    numblocks = info->chipsize >> info->phys_erase_shift;
    if (td->options & NAND_BBT_LASTBLOCK) {
        bbt_start = numblocks - td->maxblocks - md->maxblocks;
        bbt_end = numblocks;
    } else {
        bbt_start = 0;
        bbt_end = td->maxblocks + md->maxblocks;
    }

    /* If this block isn't relocated one, check it directly */
    if ((bbt_start <= block && block < bbt_end) ||
        (block >= numblocks))
        return 0;

    if (((info->bbt[block >> 2] >> ((block & 0x03) << 1)) & 0x03) != 0x00) {
        printf("block 0x%x is bad block.\n", block);
        printf("0x%x\n", info->bbt[block >> 2]);
        return 1;
    }
    return 0;
}

/* set bad block flag in memory based BBT */
static inline void set_bbt(u32 block, u8 val, struct nand_info *info)
{
    val &= 0x3;
    info->bbt[block >> 0x2] &= ~(0x3 << ((block & 0x3) << 1));
    info->bbt[block >> 0x2] |= val << ((block & 0x3) << 1);
}

/* get bad block flag from memory based BBT */
static inline void get_bbt(u32 block, u8 *pval, struct nand_info *info)
{
    *pval = (info->bbt[block >> 0x2] >> ((block & 0x3) << 1)) & 0x3;
}

/* Mark a bad block in memory based BBT and write it to flashed based one.
 * On MOBM V3, relocation table should also be updated.
 * return:
 *  -ERELOCATED: relocate successfully
 *  -EBADBLOCK: marked as bad block
 *  -ERR:  fail to mark bad block
 */
static int mark_badblock(u32 block, struct nand_info *info)
{
    int pagesize, page, i;
    u8 buf[BUFLEN];

    if (block >= (info->chipsize >> info->phys_erase_shift))
        return -ERR;

    /* mark bad block in spare area */
    pagesize = 1 << info->page_shift;
    memset(buf, 0xFF, BUFLEN);
    for (i = 0; i < info->badblockbytes; i++)
        buf[pagesize + info->badblockpos + i] = 0;

    page = block << (info->phys_erase_shift - info->page_shift);
    /* doesn't modify the ECC
     * Set the first and second page of the block as bad.
     * Avoid to erase it before write the bad block information.
     */
    mhn_write_one_page(info, buf, page, 0);
    mhn_write_one_page(info, buf, page + 1, 0);

    if (update_rel_table(info, block)){
        set_bbt(block, BBT_BLOCK_BAD, info);

        if (info->td)
            write_bbt(info, buf, info->td, info->md);
        if (info->md) {
            if (info->md->options & NAND_BBT_LASTBLOCK)
                info->md->pages = info->td->pages - (info->td->maxblocks
                        << (info->phys_erase_shift - info->page_shift));
            else
                info->md->pages = info->td->pages + (info->td->maxblocks
                        << (info->phys_erase_shift - info->page_shift));
            write_bbt(info, buf, info->md, info->td);
        }

        return -EBADBLOCK;
    }else{
        return -ERELOCATED;
    }
}

/* check whether a page and spare area is empty
 * size should be sum of page size and spare area length
 */
static inline int is_buf_blank(u8 * buf, int size)
{
    int i = 0;
    while (i < size) {
        if (*((unsigned long *)(buf + i)) != 0xFFFFFFFF)
            return 0;
        i += 4;
    }
    if (i > size) {
        i -= 4;
        while (i < size) {
            if (*(buf + i) != 0xFF)
                return 0;
            i++;
        }
    }
    return 1;
}

/* read one page */
int mhn_read_one_page(struct nand_info * info, u8 *buf, u32 page, int option)
{
    int block, reloc_block;
    int pagesize, pages_shift;
    int len;
    XLLP_UINT32_T eccpage = 0xFFFFFFFF; /* ecc error page number */
    XLLP_STATUS_T status;
    int hwecc = (option == READ_OOB);

    /* page means the page number
     * If current block is a bad block, relocate the page
     * to a correct one in a relocated block
     */
    pages_shift = info->phys_erase_shift - info->page_shift;
    block = page >> pages_shift;
    if (option != READ_RAW && is_bad_block(block, info))
        return -EBADBLOCK;

    reloc_block = search_rel_block(block, info);
    if (option != READ_RAW && reloc_block) {
        page = (reloc_block << pages_shift) + (page
            & ((1 << pages_shift) - 1));
    }
    pagesize = 1 << info->page_shift;
    memset(buf, 0xFF, pagesize + info->oobsize);
    
    enable_hw_ecc(hwecc, info);
    /* In XLLP code, len is checked restrictly.
     * So it must be set accurately.
     */
    if (hwecc)
        len = pagesize + info->oobsize - info->eccbytes;
    else
        len = pagesize + info->oobsize;
    status = XllpFlashDfcRead(info->pcontext, buf, len, page, 1, NAND_CMD_TIMEOUT,
                    &eccpage);
    enable_hw_ecc(1, info);
    
    //PRINT_BUF(buf, pagesize + info->oobsize);
    if ((status != XLLP_STATUS_SUCCESS) && ((eccpage != page) ||
        !is_buf_blank(buf, pagesize))) {
        DPRINTF("monahans_read::%d, 0x%x, %x\n", status, page,
            eccpage);
        return -1;
    }
    return 0;
}

/* erase one block, buf length must be larger than info->pagesize
 * + info->oobsize
 */
int mhn_erase_one_block(struct nand_info * info, u32 block)
{
    int reloc_block;
    XLLP_STATUS_T status;
    XLLP_UINT32_T badblock = 0xFFFFFFFF;
    u32 start;
    
    if (is_bad_block(block, info))
        return -EBADBLOCK;

    reloc_block = search_rel_block(block, info);
    if (reloc_block)
        start = reloc_block;
    else
        start = block;

    status = XllpFlashDfcErase(info->pcontext, start, 1, NAND_CMD_TIMEOUT, &badblock);

    if (status != XLLP_STATUS_SUCCESS)
        DPRINTF("monahans_erase::%d, 0x%x, %x\n", status,
            block << info->phys_erase_shift, badblock);
    if (badblock == start) {
        /* If there's an relocated block, return 0.
         * Otherwise, return error.
         */
        return mark_badblock(block, info);
    }

    if (status != XLLP_STATUS_SUCCESS)
        return  -ECMDFAIL;

    return 0;
}

int mhn_write_one_page(struct nand_info *info, u8 * buf, u32 page, int hwecc)
{
    int reloc_block, block, start;
    int pages_shift;
    XLLP_STATUS_T status;
    XLLP_UINT32_T badblock = 0xFFFFFFFF;
    
    pages_shift = info->phys_erase_shift - info->page_shift;
    /* calculate the page address with relocated block, if exists */
    block = page >> pages_shift;

    if (is_bad_block(block, info))
        return -EBADBLOCK;

    reloc_block = search_rel_block(block, info);
    if (reloc_block) {
        //DPRINTF("relocate page %x ", page);
        page = (reloc_block << pages_shift) + (page
            & ((1 << pages_shift) - 1));
        //DPRINTF(" to %x\n", page);
        start = reloc_block;
    }else {
        start = block;
    }

    enable_hw_ecc(hwecc, info);
    status = XllpFlashDfcWrite(info->pcontext, buf, page, 1, NAND_CMD_TIMEOUT, &badblock);
    enable_hw_ecc(1, info);

    if (status)
        DPRINTF("monahans_write::%d, 0x%x, %x\n", status,
            page << info->page_shift, badblock);
    if (badblock == start) {
        /* If there's an relocated block, return 0.
         * Otherwise, return error.
         */
         return mark_badblock(block, info);
    }

    if (status != XLLP_STATUS_SUCCESS)
        return -ECMDFAIL;

    return 0;
}

static int write_bbt(struct nand_info *info, u8 *buf,
            struct nand_bbt_descr *td, struct nand_bbt_descr *md)
{
    int i, j, ret, time = 0;
    int startblock = 0, dir = 0, page, numblocks, block = 0, blocks_in_page;
    int pagesize;
    u8 msk[4];
    u8 rcode = td->reserved_block_code;

    if (!rcode)
        rcode = 0xff;
    numblocks = info->chipsize >> info->phys_erase_shift;
    
    /* There was already a version of the table, reuse the page
     * This applies for absolute placement too, as we have the
     * page nr. in td->pages.
     */
    if (td->pages != -1) {
        page = td->pages;
        goto write;
    }

    /* Automatic placement of the bad block table */
    /* Search direction top -> down ? */
    if (td->options & NAND_BBT_LASTBLOCK) {
        startblock = numblocks - 1;
        dir = -1;
    } else {
        startblock = 0;
        dir = 1;
    }

find_block:
    for (i = 0; i < td->maxblocks; i++) {
        u8 val;

        block = startblock + dir * i;
        get_bbt(block, &val, info);
        /* Check, if the block is bad */
        switch (val) {
            case 0x01:
            case 0x03:
                continue;
        }
        page = block << (info->phys_erase_shift - info->page_shift);
        /* Check, if the block is used by the mirror table */
        //printf("md->pages 0x%x\n", md->pages);
        if (!md || md->pages != page) {
            //printf("erase the block\n");
            ret = mhn_erase_one_block(info, block);
            if (ret) {
                if (ret == -EBADBLOCK) {
                    continue;
                } else if (ret == -ERELOCATED){
                    goto write;
                }
                return ret;
            }
            goto write;
        }
    }
    printf("No space left to write bad block table\n");
    return -1;
write:

    /* Set up shift count and masks for the flash table */
    msk[0] = 0x00;
    msk[1] = 0x01;
    msk[2] = ~rcode;
    msk[3] = 0x03;
        
    /* Calc length */
    pagesize = 1 << info->page_shift;
    blocks_in_page = (pagesize << 3) >> 1;
    
    ret = mhn_erase_one_block(info, page >> (info->phys_erase_shift - info->page_shift));
    while (numblocks) {
        u8 *bbt = &(info->bbt[pagesize * time]);
    
        /* Preset the buffer with 0xff */
        memset(buf, 0xff, pagesize + info->oobsize);
        if (time == 0) {
            /* Pattern is located in oob area of first page */
            for (i = 0; i < 4; i++) {
                DPRINTF("%x ", td->pattern[i]);
            }
            DPRINTF("\n");
            memcpy(&buf[pagesize + td->offs], td->pattern, td->len);
            buf[pagesize + td->veroffs] = td->version;
        }
        //DPRINTF("write bbt :%x\n", time);
        /* walk through the memory table */
        for (i = 0; (i < numblocks) && (i < blocks_in_page); ) {
            u8 dat = bbt[i >> 2];
            
            for (j = 0; j < 4; j++, i++) {
                buf[i >> 2] &= ~(msk[dat & 0x03] << ((i & 0x3) << 1));
                dat >>= 2;
            }
        }
        //PRINT_BUF(buf, BUFLEN);
        ret = mhn_write_one_page(info, buf, page + time, 1);
        if (ret) {
            DPRINTF("write bbt error: ret:%d\n", ret);
            if (ret == -EBADBLOCK) {
                set_bbt(block, BBT_BLOCK_REV1, info);
                goto find_block;
            }
            return ret;
        }
        time++;
        numblocks -= blocks_in_page;
        if (numblocks < 0)
            numblocks = 0;
    }

    td->pages = page;
    DPRINTF("Bad block table written to page 0x%x, version %d\n", td->pages, td->version);
    return 0;
}

static int check_pattern(u8 *buf, struct nand_bbt_descr *td)
{
    int i;
    u8 *p = buf;
    for (i = 0; i < td->len; i++) {
        //printf("buf:%x pattern:%x\n", p[td->offs + i], td->pattern[i]);
        if (p[td->offs + i] != td->pattern[i])
            return -1;
    }
    return 0;
}

int read_bbt(struct nand_info *info, u8 *buf, struct nand_bbt_descr *td)
{
    u8 dat, tmp;
    int i = 0, j, block = 0, from = 0, ret;
    int pagesize, len;
    int totlen; /* BBT length */

    /* Read the primary version, if available */
    ret = mhn_read_one_page(info, buf, td->pages, READ_OOB);
    if (ret) {
        DPRINTF("read bbt error\n");
        return ret;
    }
    pagesize = 1 << info->page_shift;
#if 0
    PRINT_BUF(buf + pagesize + td->offs, td->len);
    PRINT_BUF(td->pattern, td->len);
#endif

    for (i = 0; i < info->badblockbytes; i++) {
        if (buf[pagesize + info->badblockpos + i] != 0xFF) {
            DPRINTF("skip this bad block\n");
            return -1;
        }
    }
    if (!check_pattern(&(buf[pagesize]), td)) {
        if (td->options & NAND_BBT_VERSION)
            td->version = buf[pagesize + td->veroffs];
    } else {
        DPRINTF("pattern isn't correct\n");
        return -1;
    }
    
    DPRINTF("BBT found at page 0x%x, version %d\n", td->pages, td->version);

        totlen = (info->chipsize >> info->phys_erase_shift) >> 2;
    
    while (totlen) {
        len = (totlen < pagesize) ? totlen : pagesize;
        
        from++;
        
        for (i = 0; i < len; i++) {
            dat = buf[i];
            for (j = 0; j < 8; j += 2, block++) {
                tmp = (dat >> j) & 0x3;
                if (tmp == 0x3)
                    continue;
                if (td->reserved_block_code && (tmp
                        == td->reserved_block_code)) {
                    set_bbt(block, BBT_BLOCK_REV2, info);
                    continue;
                }
                /* Factory marked bad or worn out ? */
                if (tmp == 0)
                    set_bbt(block, BBT_BLOCK_BAD, info);
                else
                    set_bbt(block, BBT_BLOCK_REV1, info);
                if (block > (info->chipsize >> info->phys_erase_shift))
                    return 0;
            }
        }
        totlen -= len;
        if (totlen) {
            memset(buf, 0, BUFLEN);
            ret = mhn_read_one_page(info, buf, td->pages + from, READ_OOB);
            if (ret) {
                memset(info->bbt, 0x0, BBT_LEN);
                return ret;
            }
        }
    }
    return 0;
}

/* create a memory based BBT */
static int create_bbt(struct nand_info *info, u8 * buf)
{
    int i, bytes, ret;
    int pages_shift, numblocks, blocks_in_page, pagesize;

    memset(info->bbt, 0, BBT_LEN);
    pages_shift = info->phys_erase_shift - info->page_shift;
    numblocks = info->chipsize >> info->phys_erase_shift;
    blocks_in_page = numblocks >> 2;
    pagesize = 1 << info->page_shift;

    if (info->td)
        numblocks -= info->td->maxblocks;
    if (info->md)
        numblocks -= info->md->maxblocks;

    for (i = 0; i < numblocks; i++) {
        ret = 0;
        /* check the first and second page */
        ret = mhn_read_one_page(info, buf, i << pages_shift, READ_OOB);
        if (ret) {
            DPRINTF("read page error at page %d when bbt is created\n",
                    i << pages_shift);
        }
        for (bytes = 0; bytes < info->badblockbytes; bytes++) {
            if (buf[pagesize + info->badblockpos + bytes]
                    != 0xFF) {
                set_bbt(i, BBT_BLOCK_BAD, info);
                DPRINTF("bad block %d\n", i);
                ret = 1;
                break;
            }
        }
        if (ret)
            continue;
        ret = mhn_read_one_page(info, buf, (i << pages_shift) + 1, READ_OOB);
        if (ret) {
            DPRINTF("read page error at page %d when bbt is created\n",
                    i << pages_shift);
        }

        for (bytes = 0; bytes < info->badblockbytes; bytes++) {
            if (buf[(1 << info->page_shift) + info->badblockpos
                    + bytes] != 0xFF) {
                set_bbt(i, BBT_BLOCK_BAD, info);
                DPRINTF("bad block %d, page:0x%x\n", i, i << pages_shift);
                break;
            }
        }
    }

    //PRINT_BUF(info->bbt, BBT_LEN);

    return 0;
}

/* search the BBT on the device */
static int search_bbt(struct nand_info *info, u8 *buf,
            struct nand_bbt_descr *td, int bbt_blocks)
{
    int i, totlen, pagesize;
    int startblock, block, dir;
    u8 *p = NULL;

    /* Search direction top -> down ? */
    if (td->options & NAND_BBT_LASTBLOCK) {
        startblock = (info->chipsize >> info->phys_erase_shift) - 1
                - bbt_blocks;
        dir = -1;
    } else {
        startblock = bbt_blocks;
        dir = 1;
    }
    
    p = info->bbt;
        totlen = (info->chipsize >> info->phys_erase_shift) >> 2;
    pagesize = 1 << info->page_shift;
    /* Scan the maximum number of blocks */
    for (i = 0; i < td->maxblocks; i++) {
        block = startblock + dir * i;
        td->version = 0;
        td->pages = block << (info->phys_erase_shift
                - info->page_shift);

        if (is_bad_block(block, info)) {
            DPRINTF("BBT is on a bad block\n");
            return -1;
        }
        if (read_bbt(info, buf, td)) {
            memset(buf, 0, BBT_LEN);
            td->pages = -1;
            return -1;
        } else {
            totlen = totlen - pagesize;
            /* BBT is loaded fully */
            if (totlen <= 0)
                break;
            /* copy BBT from buf to bbt[] */
            DPRINTF("copy time: %d\n", i);
            PRINT_BUF(buf, 1 << info->page_shift);
            memcpy(p, buf, 1 << info->page_shift);
            p += 1 << info->page_shift;
        }
    }
    //PRINT_BUF(info->bbt, (1 << info->page_shift) << 1);
    return 0;
}

int check_create(struct nand_info *info, u8 *buf, struct nand_bbt_descr *td,
        struct nand_bbt_descr *md)
{
    int  ret;
    int writeops = 0;
    struct nand_bbt_descr *rd, *rd2;

    
    rd = NULL;
    rd2 = NULL;
    /* Mirrored table avilable ? */
    if (md) {
        if (td->pages == -1 && md->pages == -1) {
            writeops = 0x03;
            goto create;
        }

        if (td->pages == -1) {
            rd = md;
            td->version = md->version;
            writeops = 1;
            goto writecheck;
        }

        if (md->pages == -1) {
            rd = td;
            md->version = td->version;
            writeops = 2;
            goto writecheck;
        }

        if (td->version == md->version) {
            rd = td;
            if (!(td->options & NAND_BBT_VERSION))
                rd2 = md;
            goto writecheck;
        }

        if (((td->version - md->version)) > 0) {
            rd = td;
            md->version = td->version;
            writeops = 2;
        }
        else {
            rd = md;
            td->version = md->version;
            writeops = 1;
        }

        goto writecheck;

    }
    else {
        if (td->pages == -1) {
            writeops = 0x01;
            goto create;
        }
        rd = td;
        goto writecheck;
    }
create:
    /* Create the table in memory by scanning the chip(s) */
    DPRINTF("writeops=%d\n", writeops);
    create_bbt(info, buf);
        
    td->version = 1;
    if (md)
        md->version = 1;
writecheck:
    DPRINTF("writeops=%d\n", writeops);
    /* Write the bad block table to the device ? */
    if ((writeops & 0x01)) {
        ret = write_bbt(info, buf, td, md);
        if (ret < 0)
            return ret;
    }

    /* Write the mirror bad block table to the device ? */
    if ((writeops & 0x02) && md) {
        if (md->options & NAND_BBT_LASTBLOCK)
            md->pages = td->pages - (td->maxblocks <<
                (info->phys_erase_shift - info->page_shift));
        else
            md->pages = td->pages + (td->maxblocks <<
                (info->phys_erase_shift - info->page_shift));
        ret = write_bbt(info, buf, md, td);
        if (ret < 0)
            return ret;
    }

    return 0;
}

void scan_nand(struct nand_info *info)
{
    int len,  bbt_blocks = 0;
    int i = 0,  ret;
    int pagesize, start_page;
    //u8 buf[BUFLEN];       /* contains pagesize + oobsize */

    memset(buf, 0, BUFLEN);
    pagesize = 1 << info->page_shift;
    /* the last page of the first block */
    start_page = (1 << (info->phys_erase_shift - info->page_shift)) - 1;
    
    /* for bbt, we have 2 bits for one block */
    len = (info->chipsize >> info->phys_erase_shift) >> 1;
    len = (len > BBT_LEN) ? BBT_LEN : len;

    ret = read_rel_table(info);
    if (ret) {
        DPRINTF("can't find relocation table\n");
        goto scan;
    } else
        DPRINTF("get relocation table\n");

scan:
    memset(&(info->bbt), 0x00, len);
    bbt_blocks = 0;
    if (info->td) {
        ret = search_bbt(info, buf, info->td, bbt_blocks);
        if (ret == 0)
            printf("Found Main Bad block table at address 0x%08x, version 0x%02X\n",
                info->td->pages * pagesize,
                info->td->version);
        else
            printf("Main Bad block table is missed\n");
        bbt_blocks += info->td->maxblocks;
    }
    if (info->md) {
        ret = search_bbt(info, buf, info->md, bbt_blocks);
        if (ret == 0)
            printf("Found Mirror Bad block table at address 0x%08x, version 0x%02X\n",
                info->md->pages * pagesize,
                info->md->version);
        else
            printf("Mirror Bad block table is missed\n");
        bbt_blocks += info->md->maxblocks;
    }

    DPRINTF("td pages:0x%x md pages:0x%x\n", info->td->pages, info->md->pages);
    /* If necessary, create BBT and write it back to device */
    check_create(info, buf, info->td, info->md);

    for (i = 0; i < (info->chipsize >> info->phys_erase_shift); i++) {
        u8 val;
        get_bbt(i, &val, info);
        switch(val) {
        case 0x1:
            printf("Block #%d is bad due to worn out\n", i);
            break;
        case 0x3:
            printf("Block #%d is bad due to factory  mark\n", i);
            break;
        }
    }

}

/*
   read from addr at flash, the read length is len. Because there may be
   bad blocks in the flash, the actual read length may less than len.
   We only read the page.
*/
static int monahans_nand_read(u8 *buf, u32 addr, u32 len, int option, struct nand_info *info)
{
    u32 block = -1;
    /* start is the beginning page */
    u32 start, pages;
    int pagesize, obm, i = 0;
    u8 * buffer = buf;
    int ret;

    if ((option != READ_RAW) && (addr > info->chipsize || (addr + len) > info->chipsize))
        return -1;

    obm = calc_obm_ver();
    pagesize = 1 << info->page_shift;
    if (addr & (pagesize - 1)) {
        addr &= ~(pagesize - 1);
        printf("address not align with page size, change to 0x%x\n", addr);
    }
    if (len & (pagesize - 1)) {
        len &= ~(pagesize - 1);
        printf("length not align with page size, change to 0x%x\n", len);
    }
    printf("Read flash from 0x%x, length 0x%x\n", addr, len);
    start = addr >> info->page_shift;
    pages = len >> info->page_shift;
    while (i < pages) {
        printf(".");
        ret = mhn_read_one_page(info, buffer, start, option);
        if (ret){
            if (ret == -EBADBLOCK){
                block = start >> (info->phys_erase_shift
                        - info->page_shift);
                start = (block + 1) << (info->phys_erase_shift
                        - info->page_shift);
                continue;
            }else{
                return (buffer - buf);
            }
        }
        buffer += pagesize;
        if (option == READ_OOB)
            buffer += info->oobsize;
        i++;
        start++;
    }
    printf("Done\n");
    return (buffer - buf);
}

int monahans_nand_erase(u32 addr, u32 len, WRITE_FUNC func,
            struct nand_info *info)
{
    /* start is the beginning block */
    int start, blocks, blocksize;
    int i = 0, ret, pagesize;
    int pages_per_block = 1 << (info->phys_erase_shift - info->page_shift);
    /* FIXME: the buf size should be info->pagesize + info->oobsize */
    u8 temp_buf[BUFLEN];
    int new_rel_num = 0;
    int max_block_num;

    blocksize = 1 << info->phys_erase_shift;
    pagesize = 1 << info->page_shift;

    printf("Erase flash from 0x%x, length 0x%x\n", addr, len);
    addr = (addr + blocksize - 1) & ~(blocksize - 1);
    len = (len + blocksize - 1) & ~(blocksize - 1);
    DPRINTF("Erase flash address is aligned to 0x%x, ", addr);
    DPRINTF("length is aligned to 0x%x\n", len);
    start = addr >> info->phys_erase_shift;
    blocks = len >> info->phys_erase_shift;
    max_block_num = info->chipsize >> info->phys_erase_shift;
    
    while (i < blocks && i < max_block_num) {
        if (is_bad_block(start, info)) {
            //update_rel_table(start, info);
            printf("\nSkip for bad block #%d\n", start);
            goto next;
        }
        printf(".");

        ret = mhn_erase_one_block(info, start);
        if (ret) {
            if (ret == -EBADBLOCK) {
                goto next;
            } else if (ret == -ERELOCATED){
                new_rel_num++;
            } else {
                return ret;
            }
        }
        /* we may need write OOB */
        memset(temp_buf, 0xFF, pagesize + info->oobsize);

        switch(func) {
            case ZIMAGE:
                break;
            case JFFS2:
                if (pagesize == 512)
                    memcpy(temp_buf + pagesize,
                        jffs2_sb_oob, info->oobsize);
                else
                    memcpy(temp_buf + pagesize,
                        jffs2_lb_oob, info->oobsize);
                /* fall down*/
            case YAFFS:
                /* FIXME: add YAFFS OOB data */
                ret = mhn_write_one_page(info, temp_buf,
                        start * pages_per_block, 0);
                if (ret) {
                    if (ret == -EBADBLOCK) {
                        goto next;
                    } else if (ret == -ERELOCATED){
                        new_rel_num++;
                    }else{
                        return ret;
                    }
                }
                break;
            case NULLFUNC:
                break;
        }
        i++;
next:
        start++;
    }
    
    if (new_rel_num || info->table_init == 0)
        ret =  sync_rel_table(info, &(info->current_slot));
    else
        ret = 0;

    printf("Done\n");

    return ret;
}

/*
   write from buf to addr of flash. The length of data for write in buf is len.
   In the blob, func defines what is writen to nand:
    zImage:     The linux kernel. OOB is not cared.
    jffs2 image:    The jffs2 file system image. OOB should be writen.
    yaffs image:    The yaffs file system image. OOB shoudl be writen.
   Because there may be bad block in flash, the actual length for writing in
   flash may be longer than len.
*/
int monahans_nand_write(u8 *buf, u32 addr, u32 len, WRITE_FUNC func,
            struct nand_info *info)
{
    u32 block = -1;
    int start, pages, pagesize;
    int i = 0, ret = -1, obm;
    int pages_shift;
    u8 *buffer = buf;
    u8 temp_buf[BUFLEN];
    int maxslot = 0, minslot = 0, rel_table_off, rel_table_len, flash_offset, rel_table_buf;
    int new_rel_num = 0;
    struct nand_bbt_descr *td = info->td;
    struct nand_bbt_descr *md = info->md;
    int numblocks;
    int bbt_start, bbt_end;

    if (addr > info->chipsize || (addr + len) > info->chipsize){
        printf("flash address range: 0x%08x~0x%08x\n", 0, info->chipsize);
        return -1;
    }

    pages_shift = info->phys_erase_shift - info->page_shift;
    pagesize = 1 << info->page_shift;
    /* addr aligns to block size for erasing */
    printf("Write flash from 0x%x, length 0x%x\n", addr, len);
    addr = (addr + pagesize - 1) & ~(pagesize - 1);
    len = (len + pagesize - 1) & ~(pagesize - 1);

    /* backup relocation table if block 0 will be erased */
    if (addr >= (1 << info->phys_erase_shift))
        goto erase_blocks;

    obm = calc_obm_ver();
    if (obm == MHN_OBM_V2) {
        /* On MOBM V2, the relocation table resides in the last page
         * of the first block.
         * Hardware ECC isn't necessary at this time.
         */
        maxslot = 1 << (info->phys_erase_shift - info->page_shift);
        minslot = maxslot - 1;
    } else if (obm == MHN_OBM_V3) {
        /* On MOBM V3, there're several relocation tables in the first
         * block.
         * One page contains one slot.
         */
        maxslot = 1 << (info->phys_erase_shift - info->page_shift);
        minslot = maxslot - MAX_BBT_SLOTS;
    }

    rel_table_off = minslot << info->page_shift;
    rel_table_len = (maxslot - minslot) << info->page_shift;
    rel_table_buf = (int)(buffer - addr + rel_table_off);
    flash_offset = rel_table_off;
    while(rel_table_len > 0){
        memset(temp_buf, 0xff, pagesize);
        ret = monahans_nand_read((void*)temp_buf , flash_offset,
                pagesize, READ_NORM, &monahans_nand);
        if (ret != pagesize){
            printf("relocation table backup failed!\n");
            return -1;
        }
        memcpy((void *)rel_table_buf, (void*)temp_buf, pagesize);
        rel_table_len -= pagesize;
        flash_offset += pagesize;
        rel_table_buf += pagesize;
    }
            
    if (addr > rel_table_off){
        len = len + (addr - rel_table_off);
        addr = rel_table_off;
    }
    if ((addr + len) < (1 << info->phys_erase_shift))
        len = (1 << info->phys_erase_shift) - addr;
erase_blocks:
    /* first erase the needed block */
    monahans_nand_erase(addr, len, func, info);

    /* write data to pages */
    start = addr >> info->page_shift;
    pages = len >> info->page_shift;
    DPRINTF("Write flash address is aligned to 0x%x,", addr);
        DPRINTF(" length is aligned to 0x%x\n", len);
    //printf("Write flash from 0x%x, length 0x%x\n", addr, len);
    /* first erase it for writing */
    numblocks = info->chipsize >> info->phys_erase_shift;
    if (td->options & NAND_BBT_LASTBLOCK) {
        bbt_start = numblocks - td->maxblocks - md->maxblocks;
        bbt_end = numblocks;
    } else {
        bbt_start = 0;
        bbt_end = td->maxblocks + md->maxblocks;
    }

    i = 0;
    while (i < pages) {
        if (block != start >> pages_shift) {
            block = start >> pages_shift;
            if (is_bad_block(block, info)) {
                start = (block + 1) << pages_shift;
                continue;
            }
            printf(".");
        }

        if ( bbt_start <= block && block < bbt_end){
            start = (block + 1) << pages_shift;
            continue;
        }

        if (block >= numblocks){
            printf("flash is full!\n");
            return -1;
        }

        if (is_buf_blank(buffer, pagesize)) {
            goto next;
        }
        memcpy(temp_buf, buffer, pagesize);
        memset(temp_buf + pagesize, 0xFF, info->oobsize);
        ret = mhn_write_one_page(info, temp_buf, start, 1);
        if (ret) {
            if (block == 0){
                printf("WARNING: failed to write block 0,"
                        "flash might be broken\n");
                return -1;
            }
            if (ret == -EBADBLOCK || ret == -ERELOCATED) {
                if (ret == -EBADBLOCK)
                    start = (block + 1) << pages_shift;
                else
                    start = block << pages_shift;
                i -= (i & ((1 << pages_shift) - 1));
                buffer -= pagesize*(i & ((1 << pages_shift) - 1));
                if (buffer < buf){
                    start = addr >> info->page_shift;
                    buffer = buf;
                    i = 0;
                }
                continue;
            }else{
                return (buffer - buf);
            }
        }

        ret = mhn_read_one_page(info, temp_buf, start, READ_OOB);
        if (ret || memcmp(buffer, temp_buf, pagesize)) {
            ret = mark_badblock(block, info);
            if(ret == -EBADBLOCK){
                start = (block + 1) << pages_shift;
                DPRINTF("block %d is bad,use next block!\n", block);
            } else if (ret == -ERELOCATED){
                new_rel_num++;
                start = block << pages_shift;
                DPRINTF("block %d is bad,use relocated block!\n", block);
            }else{
                return (buffer - buf);
            }
            i -= (i & ((1 << pages_shift) - 1));
            buffer -= pagesize*(i & ((1 << pages_shift) - 1));
            continue;
        }

next:
        buffer += pagesize;
        start++;
        i++;
    }

    if (new_rel_num || info->table_init == 0)
        ret =  sync_rel_table(info, &(info->current_slot));

    printf("Done\n");
    if (ret)
        return -1;
    else
        return (buffer - buf);
}

int fls(int r)
{
      int count = 0;
      int s = 1;
      while(!(r&s)){
        s = s<<1;
        count++;
      }
      return count;
}

static int DfcSetGPIO()
{
    *(u32*)(PIN_nCS3) = 0x801;
    *(u32*)(PIN_DF_nCS0_MFPR)        = 0x00001801;
    *(u32*)(PIN_DF_nCS1_MFPR)        = 0x00001801;
    *(u32*)(PIN_DF_IO0_MFPR)         = 0x00001401;
    *(u32*)(PIN_DF_IO8_MFPR)         = 0x00001401;
    *(u32*)(PIN_DF_IO1_MFPR)         = 0x00001401;
    *(u32*)(PIN_DF_IO9_MFPR)         = 0x00001401;
    *(u32*)(PIN_DF_IO2_MFPR)         = 0x00001401;
    *(u32*)(PIN_DF_IO10_MFPR)        = 0x00001401;
    *(u32*)(PIN_DF_IO3_MFPR)         = 0x00001401;
    *(u32*)(PIN_DF_IO11_MFPR)        = 0x00001401;
    *(u32*)(PIN_DF_IO4_MFPR)         = 0x00001401;
    *(u32*)(PIN_DF_IO12_MFPR)        = 0x00001401;
    *(u32*)(PIN_DF_IO5_MFPR)         = 0x00001401;
    *(u32*)(PIN_DF_IO13_MFPR)        = 0x00001401;
    *(u32*)(PIN_DF_IO6_MFPR)         = 0x00001401;
    *(u32*)(PIN_DF_IO14_MFPR)        = 0x00001401;
    *(u32*)(PIN_DF_IO7_MFPR)         = 0x00001401;
    *(u32*)(PIN_DF_IO15_MFPR)        = 0x00001401;
    
    *(u32*)(PIN_DF_nWE_MFPR)         = 0x1901;
    *(u32*)(PIN_DF_INT_RnB_MFPR)     = 0x1900;
    *(u32*)(PIN_DF_CLE_nOE_MFPR)     = 0x1900;

    *(u32*)(PIN_DF_nRE_MFPR)         = 0x1901;
    *(u32*)(PIN_DF_ALE_nWE_MFPR)     = 0x1901;

    return 0;
}

void init_hardware()
{
    *(unsigned int*)GPIO3_MFPR = 0x801;
    *(unsigned int*)GPIO4_MFPR = 0x801;

    CSADRCFG3 = 0x003e080b;
    MSC1 = 0x7ffc0000 | (MSC1 & 0xffff);
    DfcSetGPIO();
}

void init_monahans_nand_driver(void)
{
    init_hardware();
    XLLP_STATUS_T status;
    XLLP_UINT8_T manufacturer, device;
    P_XLLP_DFC_FLASH_SPECS_T pFlashInfo = NULL;
    int i = XLLP_DFC_FLASH_NULL + 1;
    //unsigned char buf[BUFLEN];

    monahans_nand.pcontext = &DFC_context;
    memset(buf, 0, BUFLEN);

    /* enable NAND clock, default is on */
    CKENA |= CKENA_4_DFC;
    while(i < XLLP_DFC_FLASH_END) {
        status = XllpFlashDfcInitNOGPIO(monahans_nand.pcontext, i);
        if (status != XLLP_STATUS_SUCCESS) {
            printf("XllpFlashDfcInitNoGPIO() failed.\n");
            return;
        }
        
        XllpFlashDfcReadID(monahans_nand.pcontext, &manufacturer,
                    &device);
        if (manufacturer == (monahans_nand.pcontext->pFlashInfo->chipID & 0xFF)
                && device == ((monahans_nand.pcontext->pFlashInfo->chipID >> 8) & 0xFF)){
            flash_config = i;
            break;
        }
        i++;
    }
    if (i == XLLP_DFC_FLASH_END) {
        printf("Flash can not be recognized. Man:0x%x, Dev:0x%x\n",
            manufacturer, device);
        return;
    }
    
    printf("\nNAND flash(Manu=0x%x Device=0x%x) detected!\n",
        manufacturer, device);

    pFlashInfo = monahans_nand.pcontext->pFlashInfo;
    monahans_nand.page_shift = fls(pFlashInfo->pageSize);
    monahans_nand.phys_erase_shift = fls(pFlashInfo->pagePerBlock
                    * pFlashInfo->pageSize);
    monahans_nand.chipsize = (pFlashInfo->numBlocks - NAND_RELOC_MAX)
                    << monahans_nand.phys_erase_shift;
    /* Only two format of pages are supported: 512 bytes and 2048 bytes */
    if (pFlashInfo->pageSize > 512) {
        monahans_nand.oobsize = 64;
        monahans_nand.eccbytes = 24;
        monahans_nand.hwecc = 24;
        monahans_nand.badblockpos = 0;
        monahans_nand.badblockbytes = 2;
        monahans_bbt_main.veroffs = 6;
        monahans_bbt_main.offs = 2;
        monahans_bbt_mirror.veroffs = 6;
        monahans_bbt_mirror.offs = 2;
    } else {
        monahans_nand.oobsize = 16;
        monahans_nand.eccbytes = 6;
        monahans_nand.hwecc = 6;
        monahans_nand.badblockpos = 5;
        monahans_nand.badblockbytes = 1;
        monahans_bbt_main.veroffs = 12;
        monahans_bbt_main.offs = 8;
        monahans_bbt_mirror.veroffs = 12;
        monahans_bbt_mirror.offs = 8;
    }
    monahans_nand.td = &monahans_bbt_main;
    /* mirror */
    monahans_nand.md = &monahans_bbt_mirror;
    
    monahans_nand.maf_id = manufacturer;
    monahans_nand.dev_id = device;
    scan_nand(&monahans_nand);
    
    return;
}

int cmd_nand_read(u32 ram_addr, u32 flash_ofs, u32 len)
{
    int ret;

    printf("Read 0x%x length data from flash: 0x%x to RAM address: 0x%x\n",
            len, flash_ofs, ram_addr);
            
    ret = monahans_nand_read((u8 *)ram_addr, flash_ofs, len, 0, &monahans_nand);

    //print_buf(ram_addr, len + monahans_nand.oobsize);

    return ret;
}

int cmd_nand_write(u32 ram_addr, u32 flash_ofs, u32 len)
{
    int ret;

    printf("Write 0x%x length data from RAM: 0x%x to flash: 0x%x\n",
        len, ram_addr, flash_ofs);
        
    ret = monahans_nand_write((u8 *)ram_addr, flash_ofs, len, ZIMAGE, &monahans_nand);

    return ret;
}

